// Copyright 2021 Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <chrono>
#include <cstring>
#include <memory>
#include <string>
#include <utility>

#include "rclcpp/rclcpp.hpp"

#include "shm_msgs/msg/point_cloud2.hpp"
#include "shm_msgs/open3d_conversions.hpp"

using namespace std::chrono_literals;
using namespace open3d::io;
using namespace shm_msgs;

class Talker : public rclcpp::Node {
private:
  using Topic = shm_msgs::msg::PointCloud2m;

public:
  explicit Talker(const rclcpp::NodeOptions &options)
      : Node("shm_open3d2m_talker", options) {

    input_cloud = CreatePointCloudFromFile("./res/pcd/2m.pcd");

    auto publishMessage = [this]() -> void {
      auto loanedMsg = m_publisher->borrow_loaned_message();

      populateLoanedMessage(loanedMsg);

      m_publisher->publish(std::move(loanedMsg));
      // We gave up ownership and loanedMsg is not supposed to be accessed
      // anymore

      m_count++;
    };

    rclcpp::QoS qos(rclcpp::KeepLast(10));
    m_publisher = this->create_publisher<Topic>("shm_open3d_2m", qos);

    // Use a timer to schedule periodic message publishing.
    m_timer = this->create_wall_timer(0.1s, publishMessage);
  }

private:
  uint64_t m_count = 1;
  rclcpp::Publisher<Topic>::SharedPtr m_publisher;
  rclcpp::TimerBase::SharedPtr m_timer;
  std::shared_ptr<open3d::geometry::PointCloud> input_cloud{std::make_shared<open3d::geometry::PointCloud>()};

  void populateLoanedMessage(rclcpp::LoanedMessage<Topic> &loanedMsg) {
    Topic &msg = loanedMsg.get();

    // Create the data.
    // In general this will not be constant.
    // Ideally we would create it in place but the ROS API does not allow
    // that. Therefore we need to copy it to the loaned message.

    // We can track a quasi dynamic (bounded) size like this to avoid
    // copying more data than needed.
    // msg.size = (uint8_t)std::min(payload.size(), (size_t)Topic::MAX_SIZE);

    // Note that msg.data is a std::array generated by the IDL compiler
    open3dToRos(*input_cloud, msg);
    msg.header.stamp = now();
    set_str(msg.header.frame_id, "shm_open3d_2m");

    RCLCPP_INFO(this->get_logger(), "Publishing ");
  }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions options;
  rclcpp::spin(std::make_shared<Talker>(options));
  rclcpp::shutdown();

  return 0;
}